#include <assert.h>
#include <math.h>
#include <float.h>
#include "PhysicsVector.h"
PhysicsVector::PhysicsVector() {
	v[0]=0;
	v[1]=0;
	v[2]=0;
}
PhysicsVector::PhysicsVector(float x, float y, float z){
	v[0] = x;
	v[1] = y;
	v[2] = z;
}
PhysicsVector::PhysicsVector(const PhysicsVector& o){
	for(int i=0;i<3;i++) v[i] = o.v[i];
}
PhysicsVector::~PhysicsVector(){ }
PhysicsVector& PhysicsVector::operator=(const PhysicsVector& o){
	if(&o != this)
		for(int i=0;i<3;i++) v[i] = o.v[i];
	return *this;
}
PhysicsVector& PhysicsVector::operator+=(const PhysicsVector& o){
	v[0]+=o.v[0];
	v[1]+=o.v[1];
	v[2]+=o.v[2];
	return *this;
}
PhysicsVector& PhysicsVector::operator-=(const PhysicsVector& o){
	v[0]-=o.v[0];
	v[1]-=o.v[1];
	v[2]-=o.v[2];
	return *this;
}
const PhysicsVector& PhysicsVector::operator-() const{
	return (PhysicsVector(0,0,0) - *this);
}
const PhysicsVector& PhysicsVector::operator+(const PhysicsVector& o) const{
	return PhysicsVector(*this)+=o;
}
const PhysicsVector& PhysicsVector::operator-(const PhysicsVector& o) const{
	return PhysicsVector(*this)-=o;
}
const PhysicsVector PhysicsVector::operator*(const float f) const {
	PhysicsVector temp = *this;
	temp.v[0] *=f;
	temp.v[1] *=f;
	temp.v[2] *=f;
	return temp;
}
const PhysicsVector PhysicsVector::operator/(const float f) const {
	PhysicsVector temp = *this;
	if(f==0) return PhysicsVector(0,0,0);
	float b = 1.0f / f;
	temp.v[0] *=b;
	temp.v[1] *=b;
	temp.v[2] *=b;
	return temp;
}
const double PhysicsVector::operator*(const PhysicsVector& o) const{
	return v[0]*o.v[0]+v[1]*o.v[1]+v[2]*o.v[2];
}
const inline PhysicsVector PhysicsVector::crossProduct(const PhysicsVector& o) const{
	return PhysicsVector(v[1]*o.v[2]-v[2]*o.v[1],v[0]*o.v[2]-v[2]*o.v[0],v[0]*o.v[1]-v[1]*o.v[0]);
}
bool PhysicsVector::operator==(const PhysicsVector& o) const{
	for(int i=0;i<3;i++) 
		if(fabs(v[i] - o.v[i]) > FLT_EPSILON) return false;	//Takes care of floating precision.
	return true;
}
bool PhysicsVector::operator!=(const PhysicsVector& o) const{
	return !((*this) == o);
}
const double PhysicsVector::norm() const{
	return sqrt(squareNorm());
}
const double PhysicsVector::squareNorm() const{
	return (*this)*(*this);
}
void PhysicsVector::normalize(){
	float squNorm = squareNorm();
	if(squNorm!=0){
		float _norm = sqrt(squNorm);
		v[0] /= _norm; v[1] /= _norm; v[2] /= _norm;
	}
	else{
		v[0] = 1.0f; v[1] = 0; v[2] = 0;
	}
}
ostream& operator<<(ostream& s, PhysicsVector& p){
	s<<p.v[0]<<" "<<p.v[1]<<" "<<p.v[2];
	return s;
}
istream& operator>>(istream& s, PhysicsVector& p){
	s>>p.v[0]>>p.v[1]>>p.v[2];
	return s;
}
const PhysicsVector operator*(const float f, const PhysicsVector p){
	return p*f;
}
const PhysicsVector operator/(const float f, const PhysicsVector p){
	return p/f;
}
float& PhysicsVector::operator[](int i){
	assert(i>-1 && i<3);
	return v[i];
}
